SPM: Extract code out of the SMC handler

Simplify the code of the SMC handler by extracting the code of
SP_EVENT_COMPLETE and MM_COMMUNICATE.

Change-Id: I9250a3f5e4b807b35c9d044592c1074a45ab9a07
Signed-off-by: Antonio Nino Diaz <[email protected]>

diff --git a/services/std_svc/spm/spm_main.c b/services/std_svc/spm/spm_main.c
index 1971516b861a0f16d1d62d4dfefc7c6057ed4e41..be76dc2dc12028496f7f3b5c163f45d58c34596e 100644 (file)
--- a/services/std_svc/spm/spm_main.c
+++ b/services/std_svc/spm/spm_main.c
@@ -161,6 +161,88 @@ int32_t spm_setup(void)
        return 0;
 }
 
+/*******************************************************************************
+ * MM_COMMUNICATE handler
+ ******************************************************************************/
+static uint64_t mm_communicate(uint32_t smc_fid, uint64_t mm_cookie,
+                              uint64_t comm_buffer_address,
+                              uint64_t comm_size_address, void *handle)
+{
+       sp_context_t *ctx = &sp_ctx;
+
+       /* Cookie. Reserved for future use. It must be zero. */
+       if (mm_cookie != 0U) {
+               ERROR("MM_COMMUNICATE: cookie is not zero\n");
+               SMC_RET1(handle, SPM_INVALID_PARAMETER);
+       }
+
+       if (comm_buffer_address == 0U) {
+               ERROR("MM_COMMUNICATE: comm_buffer_address is zero\n");
+               SMC_RET1(handle, SPM_INVALID_PARAMETER);
+       }
+
+       if (comm_size_address != 0U) {
+               VERBOSE("MM_COMMUNICATE: comm_size_address is not 0 as recommended.\n");
+       }
+
+       /* Save the Normal world context */
+       cm_el1_sysregs_context_save(NON_SECURE);
+
+       /* Wait until the Secure Partition is IDLE and set it to BUSY. */
+       sp_state_wait_switch(ctx, SP_STATE_IDLE, SP_STATE_BUSY);
+
+       /* Jump to the Secure Partition. */
+       spm_sp_prepare_enter(ctx);
+
+       SMC_RET4(&(ctx->cpu_ctx), smc_fid, comm_buffer_address,
+                comm_size_address, plat_my_core_pos());
+}
+
+/*******************************************************************************
+ * SP_EVENT_COMPLETE_AARCH64 handler
+ ******************************************************************************/
+static uint64_t sp_event_complete(uint64_t x1)
+{
+       sp_context_t *ctx = &sp_ctx;
+
+       /* Save secure state */
+       cm_el1_sysregs_context_save(SECURE);
+
+       if (ctx->state == SP_STATE_RESET) {
+               /*
+                * SPM reports completion. The SPM must have initiated the
+                * original request through a synchronous entry into the secure
+                * partition. Jump back to the original C runtime context.
+                */
+               spm_secure_partition_exit(ctx->c_rt_ctx, x1);
+
+               /* spm_secure_partition_exit doesn't return */
+       }
+
+       /*
+        * This is the result from the Secure partition of an earlier request.
+        * Copy the result into the non-secure context and return to the
+        * non-secure state.
+        */
+
+       /* Mark Secure Partition as idle */
+       assert(ctx->state == SP_STATE_BUSY);
+
+       sp_state_set(ctx, SP_STATE_IDLE);
+
+       /* Get a reference to the non-secure context */
+       cpu_context_t *ns_cpu_context = cm_get_context(NON_SECURE);
+
+       assert(ns_cpu_context != NULL);
+
+       /* Restore non-secure state */
+       cm_el1_sysregs_context_restore(NON_SECURE);
+       cm_set_next_eret_context(NON_SECURE);
+
+       /* Return to non-secure world */
+       SMC_RET1(ns_cpu_context, x1);
+}
+
 /*******************************************************************************
  * Secure Partition Manager SMC handler.
  ******************************************************************************/
@@ -173,7 +255,6 @@ uint64_t spm_smc_handler(uint32_t smc_fid,
                         void *handle,
                         uint64_t flags)
 {
-       cpu_context_t *ns_cpu_context;
        unsigned int ns;
 
        /* Determine which security state this SMC originated from */
@@ -194,43 +275,7 @@ uint64_t spm_smc_handler(uint32_t smc_fid,
                        SMC_RET1(handle, SPM_VERSION_COMPILED);
 
                case SP_EVENT_COMPLETE_AARCH64:
-                       /* Save secure state */
-                       cm_el1_sysregs_context_save(SECURE);
-
-                       if (sp_ctx.state == SP_STATE_RESET) {
-                               /*
-                                * SPM reports completion. The SPM must have
-                                * initiated the original request through a
-                                * synchronous entry into the secure
-                                * partition. Jump back to the original C
-                                * runtime context.
-                                */
-                               spm_secure_partition_exit(sp_ctx.c_rt_ctx, x1);
-
-                               /* spm_secure_partition_exit doesn't return */
-                       }
-
-                       /* Mark Secure Partition as idle */
-                       assert(sp_ctx.state == SP_STATE_BUSY);
-
-                       sp_state_set(&sp_ctx, SP_STATE_IDLE);
-
-                       /*
-                        * This is the result from the Secure partition of an
-                        * earlier request. Copy the result into the non-secure
-                        * context and return to the non-secure state.
-                        */
-
-                       /* Get a reference to the non-secure context */
-                       ns_cpu_context = cm_get_context(NON_SECURE);
-                       assert(ns_cpu_context != NULL);
-
-                       /* Restore non-secure state */
-                       cm_el1_sysregs_context_restore(NON_SECURE);
-                       cm_set_next_eret_context(NON_SECURE);
-
-                       /* Return to normal world */
-                       SMC_RET1(ns_cpu_context, x1);
+                       return sp_event_complete(x1);
 
                case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
                        INFO("Received SP_MEMORY_ATTRIBUTES_GET_AARCH64 SMC\n");
@@ -267,44 +312,7 @@ uint64_t spm_smc_handler(uint32_t smc_fid,
 
                case MM_COMMUNICATE_AARCH32:
                case MM_COMMUNICATE_AARCH64:
-               {
-                       uint64_t mm_cookie = x1;
-                       uint64_t comm_buffer_address = x2;
-                       uint64_t comm_size_address = x3;
-
-                       /* Cookie. Reserved for future use. It must be zero. */
-                       if (mm_cookie != 0U) {
-                               ERROR("MM_COMMUNICATE: cookie is not zero\n");
-                               SMC_RET1(handle, SPM_INVALID_PARAMETER);
-                       }
-
-                       if (comm_buffer_address == 0U) {
-                               ERROR("MM_COMMUNICATE: comm_buffer_address is zero\n");
-                               SMC_RET1(handle, SPM_INVALID_PARAMETER);
-                       }
-
-                       if (comm_size_address != 0U) {
-                               VERBOSE("MM_COMMUNICATE: comm_size_address is not 0 as recommended.\n");
-                       }
-
-                       /* Save the Normal world context */
-                       cm_el1_sysregs_context_save(NON_SECURE);
-
-                       /*
-                        * Wait until the state of the Secure Partition is IDLE
-                        * and set it to BUSY
-                        */
-                       sp_state_wait_switch(&sp_ctx,
-                                            SP_STATE_IDLE, SP_STATE_BUSY);
-
-                       /* Jump to the Secure Partition. */
-
-                       spm_sp_prepare_enter(&sp_ctx);
-
-                       SMC_RET4(&(sp_ctx.cpu_ctx), smc_fid,
-                                comm_buffer_address, comm_size_address,
-                                plat_my_core_pos());
-               }
+                       return mm_communicate(smc_fid, x1, x2, x3, handle);
 
                case SP_MEMORY_ATTRIBUTES_GET_AARCH64:
                case SP_MEMORY_ATTRIBUTES_SET_AARCH64: